/*
	Copyright (c) 2010 <copyright holders>
	This source code is licensed under The MIT License,
	see the file LICENSE for details.
	*/

#ifndef SPATIAL_HPP
#define SPATIAL_HPP

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <google/protobuf/stubs/common.h>
#include <iterator>
#include <list>
#include <vector>

#include "HelpDefs.hpp"

#if defined(DISABLE_SIMINT_RANGE_CHECKS) || defined(NDEBUG)
#define SIMINT_RANGE_CHECKS_DISABLED
#endif

namespace spelprog2 {
namespace sim {

typedef google::protobuf::int32 int32;
typedef google::protobuf::int64 int64;
typedef int64 WideInt;


const int32 INT32_MAX_ = +int32(0x7fffffffL);
const int32 INT32_MIN_ = -INT32_MAX_ - 1;
const int64 INT64_MAX_ = +int64(0x7fffffffffffffffLL);
const int64 INT64_MIN_ = -INT64_MAX_ - 1;


const WideInt WIDE_INT_MIN = INT64_MIN_;
const WideInt WIDE_INT_MAX = INT64_MAX_;

const int32 SIM_INT_PI = 1 << 14;

// Int class ==================================================================

/**
    This class acts as a range checked 32 bit integer. The range checks are
    only active when NDEBUG isn't defined, i.e. in debug mode.
    Boolean operators are as of yet undefined as this class' intended use does
    not encompass boolean operations.
    */
class Int {
friend bool operator== (const Int lhs, const Int rhs);
friend bool operator!= (const Int lhs, const Int rhs);
friend bool operator< (const Int lhs, const Int rhs);
friend bool operator> (const Int lhs, const Int rhs);
friend bool operator<= (const Int lhs, const Int rhs);
friend bool operator>= (const Int lhs, const Int rhs);
friend Int operator+ (const Int lhs, const Int rhs);
friend Int operator- (const Int lhs, const Int rhs);
friend Int operator* (const Int lhs, const Int rhs);
friend Int operator/ (const Int lhs, const Int rhs);
friend Int operator% (const Int lhs, const Int rhs);
friend Int operator>> (const Int lhs, const Int rhs);
friend Int operator<< (const Int lhs, const Int rhs);
friend Int operator+ (const Int op);
friend Int operator- (const Int op);
friend WideInt safeMultiply(const Int lhs, const Int rhs);

    int32 val_;

    //! Private method for doing range checked assignment.
    inline void set(WideInt val)
    {
        val_ = static_cast<int32>(val);
#ifndef SIMINT_RANGE_CHECKS_DISABLED
        assert(val <= INT32_MAX && val >= INT32_MIN);
#endif
    }
public:
    //! Default inited Int (0).
    inline Int() : val_(0) { }

    //! Simply constructs from a 32 bit integer.
    inline Int(int32 val) : val_(val) { }

    //! Converts from a 64 bit to a 32 bit integer.
    inline Int(WideInt val) : val_(static_cast<int32>(val))
    {
#ifndef SIMINT_RANGE_CHECKS_DISABLED
        assert(val <= INT32_MAX && val >= INT32_MIN);
#endif
    }

    //! Constructs from a size_t unsigned integer - useful when dealing with std containers
    inline Int(size_t val) { set(val); }

    inline Int(float val) : val_(static_cast<int32>(val + 0.5f)) { }

    inline Int(double val) : val_(static_cast<int32>(val + 0.5)) { }

    template <typename T>
    inline operator T () const
    {
        return static_cast<T>(val_);
    }

    Int abs() const         { return Int(val_ < 0? -val_ : val_); }
    Int sqrt() const        { return Int(std::sqrt(float(val_))); }
    Int safeSqrt() const    { return Int(std::sqrt(double(val_))); }

    inline Int sin(Int bias) const
    {
        return Int(float(bias) * std::sin(float(val_) / float(SIM_INT_PI) * FLOAT_PI));
    }

    inline Int cos(Int bias) const
    {
        return Int(float(bias) * std::cos(float(val_) / float(SIM_INT_PI) * FLOAT_PI));
    }

    inline Int tan(Int bias) const
    {
        return Int(float(bias) * std::tan(float(val_) / float(SIM_INT_PI) * FLOAT_PI));
    }

#ifndef SIMINT_RANGE_CHECKS_DISABLED
    inline Int operator+= (const Int rhs)  { set(WideInt(val_) +  WideInt(rhs.val_)); return *this; }
    inline Int operator-= (const Int rhs)  { set(WideInt(val_) -  WideInt(rhs.val_)); return *this; }
    inline Int operator*= (const Int rhs)  { set(WideInt(val_) *  WideInt(rhs.val_)); return *this; }
    inline Int operator<<= (const Int rhs) { set(WideInt(val_) << WideInt(rhs.val_)); return *this; }
#else
    inline Int operator+= (const Int rhs)  { val_ += rhs.val_; return *this; }
    inline Int operator-= (const Int rhs)  { val_ -= rhs.val_; return *this; }
    inline Int operator*= (const Int rhs)  { val_ *= rhs.val_; return *this; }
    inline Int operator<< (const Int rhs)  { val_ <<= rhs.val_; return *this; }
#endif

    inline Int operator/= (const Int rhs)  { val_ /= rhs.val_; return *this; }
    inline Int operator%= (const Int rhs)  { val_ %= rhs.val_; return *this; }
    inline Int operator>>= (const Int rhs) { val_ >>= rhs.val_; return *this; }
}; //class Int

inline bool operator== (const Int lhs, const Int rhs)  { return lhs.val_ == rhs.val_; }
inline bool operator!= (const Int lhs, const Int rhs)  { return lhs.val_ != rhs.val_; }
inline bool operator< (const Int lhs, const Int rhs)   { return lhs.val_ <  rhs.val_; }
inline bool operator> (const Int lhs, const Int rhs)   { return lhs.val_ >  rhs.val_; }
inline bool operator<= (const Int lhs, const Int rhs)  { return lhs.val_ <= rhs.val_; }
inline bool operator>= (const Int lhs, const Int rhs)  { return lhs.val_ >= rhs.val_; }

#ifndef SIMINT_RANGE_CHECKS_DISABLED
inline Int operator+ (const Int lhs, const Int rhs)    { return Int(WideInt(lhs.val_) +  WideInt(rhs.val_)); }
inline Int operator- (const Int lhs, const Int rhs)    { return Int(WideInt(lhs.val_) -  WideInt(rhs.val_)); }
inline Int operator* (const Int lhs, const Int rhs)    { return Int(WideInt(lhs.val_) *  WideInt(rhs.val_)); }
inline Int operator<< (const Int lhs, const Int rhs)   { return Int(WideInt(lhs.val_) << WideInt(rhs.val_)); }
#else
inline Int operator+ (const Int lhs, const Int rhs)    { return Int(lhs.val_ +  rhs.val_); }
inline Int operator- (const Int lhs, const Int rhs)    { return Int(lhs.val_ -  rhs.val_); }
inline Int operator* (const Int lhs, const Int rhs)    { return Int(lhs.val_ *  rhs.val_); }
inline Int operator<< (const Int lhs, const Int rhs)   { return Int(lhs.val_ << rhs.val_); }
#endif

inline Int operator/ (const Int lhs, const Int rhs)    { return Int(lhs.val_ /  rhs.val_); }
inline Int operator% (const Int lhs, const Int rhs)    { return Int(lhs.val_ %  rhs.val_); }
inline Int operator>> (const Int lhs, const Int rhs)   { return Int(lhs.val_ >> rhs.val_); }
inline Int operator+ (const Int op)                    { return Int(op.val_); }
inline Int operator- (const Int op)                    { return Int(-op.val_); }

inline WideInt safeMultiply(const Int lhs, const Int rhs)  { return WideInt(lhs.val_) * WideInt(rhs.val_); }

// Some popular Int constants. //TODO: find another place for them ============

const Int ANGULAR_UNITS_PER_HALFCIRCLE = SIM_INT_PI;
const Int ANGULAR_UNITS_PER_CIRCLE = Int(2) * ANGULAR_UNITS_PER_HALFCIRCLE;
const Int SCENE_TO_WORLD_SCALER = 256;
const Int NET_FRAME_RATE = 3;
const Int SIMULATION_FRAMES_PER_NET_FRAME = 10;
const Int SIMULATION_FRAME_RATE = NET_FRAME_RATE * SIMULATION_FRAMES_PER_NET_FRAME;

// Int2d class ================================================================

//TODO: implement vector-like operators
struct Int2d {
    Int x;
    Int y;

    inline Int2d() : x(), y() { }
    inline Int2d(Int x1, Int y1) : x(x1), y(y1) { }

    inline WideInt squaredLength() const    { return safeMultiply(x, x) + safeMultiply(y, y); }
    inline Int length() const           { return Int(std::sqrt(double(squaredLength()))); }

    inline Int2d operator+= (const Int rhs)     { x += rhs; y += rhs; return *this; }
    inline Int2d operator-= (const Int rhs)     { x -= rhs; y -= rhs; return *this; }
    inline Int2d operator*= (const Int rhs)     { x *= rhs; y *= rhs; return *this; }
    inline Int2d operator/= (const Int rhs)     { x /= rhs; y /= rhs; return *this; }

    inline Int2d operator+= (const Int2d & rhs) { x += rhs.x; y += rhs.y; return *this; }
    inline Int2d operator-= (const Int2d & rhs) { x -= rhs.x; y -= rhs.y; return *this; }

    inline void normalize(Int bias)
    {
        WideInt len = length();
        x = WideInt(x) * WideInt(bias) / len;
        y = WideInt(y) * WideInt(bias) / len;
    }

    inline Int atan2() const
    {
        return Int(float(ANGULAR_UNITS_PER_HALFCIRCLE) * std::atan2(float(y), float(x)) / FLOAT_PI);
    }
};

inline bool operator== (const Int2d & lhs, const Int2d & rhs)   { return lhs.x == rhs.x && lhs.y == rhs.y; }
inline bool operator!= (const Int2d & lhs, const Int2d & rhs)   { return lhs.x != rhs.x || lhs.y != rhs.y; }

inline Int2d operator+ (const Int2d & lhs, const Int rhs)       { return Int2d(lhs.x + rhs, lhs.y + rhs); }
inline Int2d operator- (const Int2d & lhs, const Int rhs)       { return Int2d(lhs.x - rhs, lhs.y - rhs); }
inline Int2d operator* (const Int2d & lhs, const Int rhs)       { return Int2d(lhs.x * rhs, lhs.y * rhs); }
inline Int2d operator/ (const Int2d & lhs, const Int rhs)       { return Int2d(lhs.x / rhs, lhs.y / rhs); }
inline Int2d operator+ (const Int lhs, const Int2d & rhs)       { return Int2d(lhs + rhs.x, lhs + rhs.y); }
inline Int2d operator- (const Int lhs, const Int2d & rhs)       { return Int2d(lhs - rhs.x, lhs - rhs.y); }
inline Int2d operator* (const Int lhs, const Int2d & rhs)       { return Int2d(lhs * rhs.x, lhs * rhs.y); }
inline Int2d operator/ (const Int lhs, const Int2d & rhs)       { return Int2d(lhs / rhs.x, lhs / rhs.y); }

inline Int2d operator+ (const Int2d & lhs, const Int2d & rhs)   { return Int2d(lhs.x + rhs.x, lhs.y + rhs.y); }
inline Int2d operator- (const Int2d & lhs, const Int2d & rhs)   { return Int2d(lhs.x - rhs.x, lhs.y - rhs.y); }

inline Int2d operator+ (const Int2d & op)   { return op; }
inline Int2d operator- (const Int2d & op)   { return Int2d(-op.x, -op.y); }

// Int3d class ================================================================

//TODO: implement vector-like operators
struct Int3d {
    Int x;
    Int y;
    Int z;

    inline Int3d() : x(), y(), z() { }
    inline Int3d(Int x1, Int y1, Int z1) : x(x1), y(y1), z(z1) { }

    inline WideInt squaredLength() const    { return safeMultiply(x, x) + safeMultiply(y, y) + safeMultiply(z, z); }
    inline Int length() const           { return Int(std::sqrt(double(squaredLength()))); }

    inline Int3d operator+= (const Int rhs)     { x += rhs; y += rhs; z += rhs; return *this; }
    inline Int3d operator-= (const Int rhs)     { x -= rhs; y -= rhs; z -= rhs; return *this; }
    inline Int3d operator*= (const Int rhs)     { x *= rhs; y *= rhs; z *= rhs; return *this; }
    inline Int3d operator/= (const Int rhs)     { x /= rhs; y /= rhs; z /= rhs; return *this; }

    inline Int3d operator+= (const Int3d & rhs) { x += rhs.x; y += rhs.y; z += rhs.z; return *this; }
    inline Int3d operator-= (const Int3d & rhs) { x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this; }

    inline void normalize(Int bias)
    {
        WideInt len = length();
        x = WideInt(x) * WideInt(bias) / len;
        y = WideInt(y) * WideInt(bias) / len;
        z = WideInt(z) * WideInt(bias) / len;
    }
};

inline bool operator== (const Int3d & lhs, const Int3d & rhs)   { return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; }
inline bool operator!= (const Int3d & lhs, const Int3d & rhs)   { return lhs.x != rhs.x || lhs.y != rhs.y || lhs.z != rhs.z; }

// SpatialHash class ==========================================================

class SpatialHash2dFunctor {
    Int cellX_;
    Int cellY_;
    Int gridWidth_;
public:
    SpatialHash2dFunctor(Int cellX, Int cellY, Int gridWidth) : cellX_(cellX), cellY_(cellY), gridWidth_(gridWidth) { }

    size_t operator() (const Int2d & key) {
        
    }
};

/**
    This spatial hash maps a rectangle in 2d space (0, 0, mapWidth, mapHeight)
    to lists of user defined values, by defining a grid of rectangle "cells"
    that covers the entire 2d input domain.
    It is possible to add and remove values, as well as finding the four
    closest cells to a certain position. (Very useful for e.g. pruning
    collision checks.)
    */
template <typename V>
struct SpatialHash2d {
    typedef std::list<V> CellList;
private:
    Int cellX_;
    Int cellY_;
    Int gridWidth_;
    Int gridHeight_;
    typedef std::vector<CellList> CellV;
    CellV cells_;

    Int hash(const Int2d & pos)
    {
        Int x = pos.x / cellX_ + 1;
        Int y = pos.y / cellY_ + 1;

        if (x < 1) {
            x = 1;
        }
        else if (x > gridWidth_ - 2) {
            x = gridWidth_ - 2;
        }
        if (y < 1) {
            y = 1;
        }
        else if (y > gridHeight_ - 2) {
            y = gridHeight_ - 2;
        }
        
        return y * gridWidth_ + x;
    }

    Int closestHash(const Int2d & pos)
    {
        Int key = hash(pos);
        if (pos.x % cellX_ <= cellX_ / 2) {
            key -= 1;
        }
        if (pos.y % cellY_ <= cellY_ / 2) {
            key -= gridWidth_;
        }

        return key;
    }
public:
    /**
        Constructs a spatial hash for a map with cells of a custom size.
        A map is considered to be the bounding rectangle
        where values may be placed. Due to the integer coordinates in
        use, for a point (x,y) it must be that
            0 <= x < mapWidth and 0 <= y < mapHeight.
        @param cellX Width of each cell.
        @param cellY Height of each cell.
        @param mapWidth Width of the "map".
        @param mapHeight Height of the "map".
        */
    SpatialHash2d(Int cellX, Int cellY, Int mapWidth, Int mapHeight) :
            cellX_(cellX),
            cellY_(cellY),
            gridWidth_(mapWidth / cellX + 3),
            gridHeight_(mapHeight / cellY + 3),
            cells_(size_t(gridWidth_ * gridHeight_))
    {
        for (Int i = 0, n = gridWidth_ * gridHeight_; i < n; i += 1) {
            cells_.push_back(CellList());
        }
        assert(cellX_ > 0);
        assert(cellY_ > 0);
        assert(gridWidth_ > 0);
        assert(gridHeight_ > 0);
    }
    
    //! Adds a value to the cell at a certain position.
    void add(const Int2d & pos, V v) {
        Int key = hash(pos);
        cells_[key].push_back(v);
    }

    //! Removes a certain value if in the cell of a certain position.
    void remove(const Int2d & pos, V v) {
        Int key = hash(pos);
        CellList & cell = cells_[key];

        typename CellList::iterator it = std::find(cell.begin(), cell.end(), v);
        if (it != cell.end()) {
            cell.erase(it);
        }
    }

    //! Removes a certain value. Inefficient, prefer to use remove(const Int2d &, V)
    void remove(V v)
    {
        for (typename CellV::iterator it = cells_.begin(), end = cells_.end(); it != end; ++it) {
            typename CellList::iterator cell = std::find((*it).begin(), (*it).end(), v);
            if (cell != (*it).end()) {
                (*it).erase(cell);
            }
        }
    }

    //! Removes all contents.
    void clear()
    {
        cells_.clear();
    }

    /** Finds all stored values at the four cells closest to a certain position.
        Invalid indices outside the grid are remapped to outer (possibly crazy) valid indices.
        Hence, always check the actual positions of the elements returned from this method.
        */
    template <typename C>
    void find(const Int2d & pos, std::back_insert_iterator<C> & out)
    {
        const Int key = closestHash(pos);

        const Int keyNW = key; //NW = northwest
        const Int keyNE = key + 1;
        const Int keySW = key + gridWidth_;
        const Int keySE = key + gridWidth_ + 1;

        std::copy(cells_[keyNW].begin(), cells_[keyNW].end(), out);
        std::copy(cells_[keyNE].begin(), cells_[keyNE].end(), out);
        std::copy(cells_[keySW].begin(), cells_[keySW].end(), out);
        std::copy(cells_[keySE].begin(), cells_[keySE].end(), out);
    }

    /** Finds all stored values at the cells that cover a rectangle with a certain center point and size.
        Invalid indices outside the grid are remapped to outer (possibly crazy) valid indices.
        Hence, always check the actual positions of the elements returned from this method.
        */
    template <typename C>
    void find(const Int2d & pos, const Int2d & size, std::back_insert_iterator<C> & out)
    {
        const Int2d halfSize = size / Int(2);

        const Int leastKey = closestHash(Int2d(pos.x - halfSize.x, pos.y - halfSize.y));
        assert(leastKey >= 0 && leastKey < gridWidth_ * gridHeight_);
        const Int mostKey = closestHash(Int2d(pos.x + halfSize.x, pos.y + halfSize.y)) + gridWidth_ + 1;
        assert(mostKey >= 0 && mostKey < gridWidth_ * gridHeight_);

        const Int diff = mostKey - leastKey;
        if (diff < 0) {
            return;
        }

        const Int pitch = diff % gridWidth_;

        for (Int i = leastKey; i <= mostKey - pitch; i += gridWidth_) {
            for (Int j = Int(0); j <= pitch; j += 1) {
                assert(i + j < gridWidth_ * gridHeight_);
                std::copy(cells_[i + j].begin(), cells_[i + j].end(), out);
            }
        }
    }

    //TODO: evaluate time cost of this, might not be acceptable, might simply be better with a global list
    //! Gets all stored values.
    template <typename C>
    void all(std::back_insert_iterator<C> & out)
    {
        for (Int i = 0, n = gridWidth_ * gridHeight_; i < n; i += 1) {
            std::copy(cells_[i].begin(), cells_[i].end(), out);
        }
    }

    //! Gets all stored values. Const version.
    template <typename C>
    void all(std::back_insert_iterator<C> & out) const
    {
        for (Int i = 0, n = gridWidth_ * gridHeight_; i < n; i += 1) {
            std::copy(cells_[i].begin(), cells_[i].end(), out);
        }
    }
};

}; //sim
}; //spelprog2

#endif //SPATIAL_HPP

